#include <math.h>
#include "distances.h"
#include "ros/ros.h"
#include "std_msgs/String.h"
//#include "std_msgs/Header.h"
#include "std_msgs/Int32.h"
#include "std_msgs/Float64MultiArray.h"
#include "pi_tracker/Skeleton.h"
#include "distances/DistancesMatrix.h"
#include "distances/VectorHeights.h"
#include <vector>
#include <iostream>
#include <opencv2/opencv.hpp>
#include "st.h"
#include <sstream>
#include "IndList.h"
#include "Display.h"


ros::NodeHandle *pn;
ros::Publisher *pp;


//altura do sensor com relação ao chão em metros
float sensorHeight = 0.667;
int isecs = -1;

float angle(const pi_tracker::Skeleton::ConstPtr msg, int ea, int c, int eb){
  
  float xea = msg->position[ea].x;
  float yea = msg->position[ea].y;
  float zea = msg->position[ea].z;
  
  float xc = msg->position[c].x;
  float yc = msg->position[c].y;
  float zc = msg->position[c].z;
  
  float xeb = msg->position[eb].x;
  float yeb = msg->position[eb].y;
  float zeb = msg->position[eb].z;
  
  Stat st;
  
  return st.angleVectors(xea-xc,yea-yc,zea-zc,xc-xeb,yc-yeb,zc-zeb);
}

float angle(float ax,float ay,float az,float cx, float cy, float cz, float bx,float by,float bz){
  
  Stat st;
  
  return st.angleVectors(ax-cx,ay-cy,az-cz,cx-bx,cy-by,cz-bz);
}

void Callback(const pi_tracker::Skeleton::ConstPtr &msg) {

  bool validData = true;
  
  if(isecs == -1) {
    isecs = msg->header.stamp.sec;
  }
  
  float heights[15];
  distances::VectorHeights vectorHeights;
  vectorHeights.v.clear();
  
  for(int i = 0 ; i < 15 ; i++) {
    heights[i] = msg->position[i].y + sensorHeight;
    vectorHeights.v.push_back( (double) (msg->position[i].y + sensorHeight) );
    //std::cout << "height of joint " << i << " = " << msg->position[i].y + sensorHeight << std::endl;
  }

  int secs = (int) msg->header.stamp.sec - isecs;
  int nsecs = (int) msg->header.stamp.nsec;
  
  int time_mi = 1000 * secs + (nsecs/1000000);
  
  vectorHeights.sensorHeight = sensorHeight;
  
  vectorHeights.time_mi = time_mi;
  
  //knee
  float rKneeAngle = angle(msg, 12,13,14);
  float eKneeAngle = angle(msg, 9,10,11);
  
  //media vetores hips
  float mHipsX = ((msg->position[12].x)+(msg->position[9].x))/2;
  float mHipsY = ((msg->position[12].y)+(msg->position[9].y))/2;
  float mHipsZ = ((msg->position[12].z)+(msg->position[9].z))/2;
  
  //angulo entre quadris e pescoço(torso é o centro)
  float torsoAngle = angle(mHipsX,mHipsY,mHipsZ,msg->position[2].x,msg->position[2].y,msg->position[2].z,msg->position[1].x,msg->position[1].y,msg->position[1].z);
  
  //media vetores joelhos
  float mKneesX = ((msg->position[13].x)+(msg->position[10].x))/2;
  float mKneesY = ((msg->position[13].y)+(msg->position[10].y))/2;
  float mKneesZ = ((msg->position[13].z)+(msg->position[10].z))/2;
  
  //angulo entre joelhos e pescoço (quadris sao o centro)
  float hipsAngle = angle(mKneesX,mKneesY,mKneesZ,mHipsX,mHipsY,mHipsZ,msg->position[1].x,msg->position[1].y,msg->position[1].z);
  
  if(torsoAngle < 170){
    std::cout<< "TORSO. theta(Torso) = "<< torsoAngle <<std::endl;
    validData = false;
    
  }
  
  if(hipsAngle < 170){
    std::cout<< "QUADRIS. theta(Hips) = "<< hipsAngle <<std::endl;
    validData = false;
    
  }
  
  if(rKneeAngle < 170){
    
    std::cout<< "JOELHO DIREITO. theta(Right Knee) = "<< rKneeAngle <<std::endl;
    validData = false;
  }
  
  if(eKneeAngle < 170){
    
    std::cout<< "JOELHO ESQUERDO. theta(Left Knee) = "<< eKneeAngle <<std::endl;
    validData = false;
  }
  
  
  
  
  
  if(validData){
    vectorHeights.validData = 1;
    
    //altura media das pernas + torso + pescoço
     vectorHeights.neckEstimatedHeight = mHipsY + 
  sqrt((msg->position[2].x - mHipsX)*(msg->position[2].x - mHipsX)+(
    msg->position[2].y - mHipsY)*(msg->position[2].y - mHipsY)+
    (msg->position[2].z - mHipsZ)*(msg->position[2].z - mHipsZ))
  + skeletonDistance(msg, 1, 2);
  
  //altura media das pernas + torso + pescoço + cabeça
     vectorHeights.headEstimatedHeight = mHipsY + 
  sqrt((msg->position[2].x - mHipsX)*(msg->position[2].x - mHipsX)+(
    msg->position[2].y - mHipsY)*(msg->position[2].y - mHipsY)+
    (msg->position[2].z - mHipsZ)*(msg->position[2].z - mHipsZ))
  + skeletonDistance(msg, 1, 2)
  + skeletonDistance(msg, 0, 1);
  
  
    pp->publish(vectorHeights);
  } else {
    vectorHeights.validData = 0;
  }
  
 
  
  
  
}



using namespace cv;

int main(int argc, char **argv) {

    using namespace std;


    /**right
     * The ros::init() function needs to see argc and argv so that it can perform
     * any ROS arguments and name remapping that were provided at the command
     * line. For programmatic
     * remappings you can use a different version of init() which takes remappings
     * directly, but for most command-line programs, passing argc and argv is the
     * easiest
     * way to do it.  The third argument to init() is the name of the node.
     *
     * You must call one of the versions of ros::init() before using any other
     * part of the ROS system.
     */
    ros::init(argc, argv, "heights");

    /**
     * NodeHandle is the main access point to communications with the ROS system.
     * The first NodeHandle constructed will fully initialize this node, and the
     * last
     * NodeHandle destructed will close down the node.
     */
    ros::NodeHandle n;
    pn = &n;

    /**
     * The subscribe() call is how you tell ROS that you want to receive messages
     * on a given topic.  This invokes a call to the ROS
     * master node, which keeps a registry of who is publishing and who
     * is subscribing.  Messages are passed to a callback function, here
     * called chatterCallback.  subscribe() returns a Subscriber object that you
     * must hold on to until you want to unsubscribe.  When all copies of the
     * Subscriber
     * object go out of scope, this callback will automatically be unsubscribed
     * from
     * this topic.
     *
     * The second parameter to the subscribe() function is the size of the message
     * queue.  If messages are arriving faster than they are being processed, this
     * is the number of messages that will be buffered up before beginning to
     * throw
     * away the oldest ones.
     */


    ros::Subscriber sub = n.subscribe("skeleton", 1000, Callback);

    ros::Publisher pub = n.advertise<distances::VectorHeights > ("heights", 1000);

    pp = &pub;



    ros::Rate loop_rate(10);


    /**
     * ros::spin() will enter a loop, pumping callbacks.  With this version, all
     * callbacks will be called from within this thread (the main one). 
     * ros::spin()	
     * will exit when Ctrl-C is pressed, or the node is shutdown by the master.
     */
    ros::spin();





    return 0;
}
